/*************************************************
*      Filename: keyframe_movingpoint.cu
*        Coding: utf-8
*        Author: Futuan Li
*         Email: lifutuan@galasports.net
*      HomePage: http://www.galasports.net
*  Copyright(C): InnoReal Technology Co.,Ltd.
*       Version: 0.0.1
*    CreateDate: 2018-04-24 15:50:58
*    LastChange: 2018-07-18 11:36:06
*          Desc: 
*       History: 
*************************************************/
#include "keyframe_movingpoint.h"
#include <innoreal/utils/innoreal_helper.hpp>
#include <innoreal/utils/innoreal_timer.hpp>
#include "innoreal/utils/innoreal_timestamp.hpp"

//input:sf_sum(总的帧的数量)，sf_given(关键帧的数量); output: keyframe(关键帧的编号)
void KeyFrameMovingPoint::KFrame_Distance(int sf_Num, int sf_given) {
  sourceframe_Num = sf_Num;
  givenframe_Num = sf_given;

  printf("Step1: Initialization in KFrame_Distance.......\n");
  Calculate_Total_Evalation *calculate_total_evalation = new Calculate_Total_Evalation;
  for(int iter_out = 0; iter_out < sourceframe_Num; iter_out++) {
    keyframe.push_back(iter_out);
  }

  calculate_total_evalation->Cal_Evalation_all(keyframe, sf_Num);
  hausdorff_dis.resize(keyframe.size());

  for(int i = 0; i < keyframe.size(); i++) {
    hausdorff_dis[i]= calculate_total_evalation->evalate_dis_one[i];
  }
  delete calculate_total_evalation;

  printf("end keyframe.......\n");

}


//KFrame_MP函数的作用是获得关键帧的编号，input:sf_sum(总的帧的数量)，sf_given(关键帧的数量); output: keyframe(关键帧的编号)
void KeyFrameMovingPoint::KFrame_MP(int sf_Num, int sf_given) {
	sourceframe_Num = sf_Num; //初始化，将输入的数据存储到类中变量
	givenframe_Num = sf_given;
	printf("Step1: Initialization .......\n");
	Calculate_Total_Evalation *calculate_total_evalation = new Calculate_Total_Evalation;
	//关键帧初始化，keyframe
	for(int iter_out = 0; iter_out < sourceframe_Num; iter_out++) {
        if( (iter_out%((sourceframe_Num+1)/givenframe_Num) == ((sourceframe_Num+1)/givenframe_Num)-1) && keyframe.size() < givenframe_Num)
			keyframe.push_back(iter_out);
	}
	//for(int i=0; i< sf_given; i++){
		//INNOREALI("%d ++++++++++++++++++++ \n", keyframe[i]);
	//}

	//关键步骤计算，存储所有的关键帧和距离结果
	calculate_total_evalation->Cal_Evalation(keyframe, sf_Num);
	// diseva双方向的记录结果
	diseva.resize(keyframe.size());
	for(int i = 0; i < keyframe.size(); i++) {
		diseva[i].resize(2);
		diseva[i][0] = calculate_total_evalation->evalate_dis[i][0];
		diseva[i][1] = calculate_total_evalation->evalate_dis[i][1];
 	}
	delete calculate_total_evalation;
  //for(int i=0; i< keyframe.size(); i++){
		//INNOREALI("TEST:::::::::%f \n", diseva[i][0]);
	//}

	//iteration by moving the point
	printf("Step2: Iterations of moving point.......\n");
	int iteration_Num = 10; // iteration_Num:移动迭代的次数，可以修改，一般要求足够多，sf_Num*sf_Num足够
	Moving_Point *moving_point = new Moving_Point;

	moving_point->Moving_Point_Auto(keyframe, diseva);//初始化，赋值一些数据，将便于后续算法的运行
	//opt 1:这里提供一种基本的策略
    /*
	for(int iter = 0; iter < iteration_Num; iter++) {
		moving_point->Moving_Point_Update(sourceframe_Num);  //opt
		printf("Start %dth Iteration: the max value of hausdorff distance is %f.\n \n", iter, moving_point->dis_max_value_font);
		for(int j = 0; j < keyframe.size(); j++) {
			keyframe[j]  = moving_point->keyframe[j];
			diseva[j][0] = moving_point->distance_eva_col0[j];
			diseva[j][1] = moving_point->distance_eva_col1[j];
		}
	}
	*/
	//opt 2: 这里提供一种较优的策略
	for(int iter = 0; iter < iteration_Num; iter++) {
		//INNOREALI(" \n testtest interface: !!! \n"); // test 1
		moving_point->Moving_TPoint_Update(sourceframe_Num);
		//printf("Start %dth Iteration: the max value of hausdorff distance is %f.\n \n", iter, moving_point->dis_max_value_font);
		for(int j = 0; j < keyframe.size(); j++) {
			keyframe[j]  = moving_point->keyframe[j];
			diseva[j][0] = moving_point->distance_eva_col0[j];
			diseva[j][1] = moving_point->distance_eva_col1[j];
		}
	}

	///*
//	//opt 3: 这里提供一种lft提供的基于移动关键帧法的最优的策略： opt 1, opt 2, opt 3均是移动关键帧的方法，一步步优化过来的
//	for(int iter = 0; iter < iteration_Num; iter++) {
//		moving_point->Moving_NTPoint_Update(sourceframe_Num);
//		printf("Start %dth Iteration: the max value of hausdorff distance is %f.\n \n", iter, moving_point->dis_max_value_font);
//		for(int j = 0; j < keyframe.size(); j++) {
//			keyframe[j]  = moving_point->keyframe[j];
//			diseva[j][0] = moving_point->distance_eva_col0[j];
//			diseva[j][1] = moving_point->distance_eva_col1[j];
//		}
//	}
 //*/
	/*
	//opt 4: 这里提供一种lft提供的基于插值的最优的策略：注意到移动点法无法保证不陷入局部极小值，插值法不同于此======>最终需要将这两种方法进行交替迭代求解
	for(int iter = 0; iter < iteration_Num; iter++) {
		moving_point->Interp_point(sourceframe_Num);  //为了方便，将此函数依旧放在moving_point.cu文件中
		for(int j = 0; j < keyframe.size(); j++) {
			keyframe[j]  = moving_point->keyframe[j];
			diseva[j][0] = moving_point->distance_eva_col0[j];
			diseva[j][1] = moving_point->distance_eva_col1[j];
		}
	}
  */


  int numIter=1, K=sf_given, N=sf_Num, startF=1;
  KeyFrameMovingPoint kfm;
  std::vector<int> Label(N,0), LabelSeries(N,0), center(K,0);
  std::vector<double> Cost(numIter,0);

  int spacing=N/K;
  for(int i=0;i<K;i++){
    //    center[i]=(i*spacing);
    center[i]=keyframe[i];
  }


  kfm.KFrame_KNN_Tool(startF, sf_Num, sf_given , numIter,
                   &center, &Label, &LabelSeries, &Cost);

  KeyFrameTools kftI;
  KeyFrameTools kftD;
  kftD.saveVector("data/168/keyframe_csv_lft/costK", K, iteration_Num, startF, Cost);
  kftI.saveVector("data/168/keyframe_csv_lft/keyframeK", K, iteration_Num, startF, LabelSeries);


  delete moving_point;

}


//设计了一个KNNTool函数来执行KNN操作,同时也可以让李福团的移动算法可以输出文件
void KeyFrameMovingPoint::KFrame_KNN_Tool(int startFrame, int sf_Num, int sf_given ,int numIter,
                                        std::vector<int>* Center, std::vector<int>* Label, std::vector<int>* LabelSeries, std::vector<double>* Cost){
  std::vector<int> center=*Center;
  int startF=startFrame; int N = sf_Num;
  int Iter=numIter;	int K = sf_given;

  std::vector<int> label(N,0);
  std::vector<double> cost(Iter,0);


  typedef double (DisTwoMesh::*DisHandle)(InputData *);
  DisHandle pmf; pmf = &DisTwoMesh::Hausdorff_Simple_par;


  InputDataKeyFrame *input_data = new InputDataKeyFrame;
  DisTwoMesh *distmesh = new DisTwoMesh;

  int c1=0, c2=0;
  std::vector<double> costL(K,0), costR(K,0);
  std::vector<int> L(K,0), R(K,0), labelSeries(N,0);
  char file_src[100], file_tar[100], file_tar1[100], file_tar2[100];
  double dis1=0,dis2=0,dis=0,d=0;

  for(int iter=0;iter<Iter;iter++){

    for(int i=0;i<K;i++){
      costL[i]=0;costR[i]=0;L[i]=0;R[i]=0;
    }

    for (int i = 0; i < center[0]; i++) {
      sprintf(file_src, "./data/168/mesh/mesh%d.obj", center[0]+startF);
      sprintf(file_tar, "./data/168/mesh/mesh%d.obj", i+startF);
      input_data->Init(file_src,file_tar);
      dis = (distmesh->*pmf)(input_data);
      label[i] = 1;
      costL[0] += dis;
    }
    L[0] = 0;

    for (int i = center.back(); i < N; i++) {
      sprintf(file_src, "./data/168/mesh/mesh%d.obj", center[K-1]+startF);
      sprintf(file_tar, "./data/168/mesh/mesh%d.obj", i+startF);
      input_data->Init(file_src,file_tar);
      dis = (distmesh->*pmf)(input_data);
      label[i] = K;
      costR[K - 1] += dis;
//      std::cout<<"KNNinTool:"<<dis<<std::endl;

    }
    R[K - 1] = N - 1;

    for (int i = 0; i < K - 1; i++) {
      c1 = center[i];
      c2 = center[i + 1];
      sprintf(file_tar1, "./data/168/mesh/mesh%d.obj", c1+startF);
      sprintf(file_tar2, "./data/168/mesh/mesh%d.obj", c2+startF);
      for (int j = c1; j <= c2; j++) {
        sprintf(file_src, "./data/168/mesh/mesh%d.obj", j+startF);
        input_data->Init(file_src,file_tar1);
        dis1 = (distmesh->*pmf)(input_data);
        input_data->Init(file_src,file_tar2);
        dis2 = (distmesh->*pmf)(input_data);
        if (dis1 < dis2) {
          label[j] = i + 1;
          costR[i] += dis1;
          R[i] = j;
        } else {
          label[j] = i + 2;
          costL[i + 1] += dis2;
          if (L[i + 1] == 0) {
            L[i + 1] = j;
          }
        }
      }
    }
    d = 0;
    for (int i = 0; i < K; i++) {
      std::cout<<costL[i]<<","<<costR[i]<<"   ";
      d += costL[i] + costR[i];
    }
    std::cout<<"CostinTool:"<<d<<std::endl;
    cost[iter]=d;

    for(int i=0;i<N;i++){
      labelSeries[i]=center[label[i]-1]+startF;
    }

    //计算下一轮的中心
    for (int i = 0; i < K; i++) {
      center[i] = ceil((L[i]+1 + R[i]+1) / 2.0)-1;
    }

  }

  *Label=label;
  *Cost=cost;
  *LabelSeries=labelSeries;

}

//调用KNN_Tool来提取关键帧
void KeyFrameMovingPoint::KFrame_KNN(int sf_Num, int sf_given) {

  int numIter=10, K=sf_given, N=sf_Num, startF=1;
  KeyFrameMovingPoint kfm;
  std::vector<int> Label(N,0), LabelSeries(N,0), center(K,0);
  std::vector<double> Cost(numIter,0);

  int spacing=N/K;
  for(int i=0;i<K;i++){
    center[i]=(i*spacing);
  }

  kfm.KFrame_KNN_Tool(startF, N, K , numIter,
                  &center, &Label, &LabelSeries, &Cost);

  KeyFrameTools kftI;
  KeyFrameTools kftD;
  kftD.saveVector("data/168/keyframe_csv2/costK", K, numIter, startF, Cost);
  kftI.saveVector("data/168/keyframe_csv2/keyframeK", K, numIter, startF, LabelSeries);
}


//最初的设计用于参考, 注释的部分主要为了尝试使用模板类来输出文件; 简化后分成了一个Tool函数还有相应的调用结构
void KeyFrameMovingPoint::KFrame_KNN_Standard(int sf_Num, int sf_given){
	InputDataKeyFrame *input_data = new InputDataKeyFrame;
	DisTwoMesh *distmesh = new DisTwoMesh;
//	KeyFrameTools<std::vector<int>> *kftI=new KeyFrameTools <std::vector<int>>;
//    KeyFrameTools<std::vector<double>> *kftD=new KeyFrameTools <std::vector<double>>;
	KeyFrameTools kftI;
	KeyFrameTools kftD;

//	KeyFrameTools<double> kftD;


    typedef double (DisTwoMesh::*DisHandle)(InputData *);
    DisHandle pmf; pmf = &DisTwoMesh::Hausdorff_Simple_par2;


    char file_src[100], file_tar[100], file_tar1[100], file_tar2[100];

	int N = sf_Num, K = sf_given;
	int Iter=4;	int startF=1;
	INNOREALI("Frame_sum: %d    Frame_given: %d \n", N, K);

	std::vector<int> center;
	std::vector<double> costL,costR;
	std::vector<int> L,R;
	int c1=0,c2=0;
	double dis1=0,dis2=0,dis=0;
	int label[N];
	std::vector<double> cost;
	double d;

	int spacing=N/K;
	for(int i=0;i<K;i++){
		center.push_back(i*spacing);
		costL.push_back(0);		costR.push_back(0);
		L.push_back(0);		R.push_back(0);
	}
//    kftD->saveVector("data/168/keyframe_csv2/keyframeK", K, Iter, 1, costL);
//	double a=1.0;
//	kftD.temp("data/168/keyframe_csv2/keyframeK", a);


  for(int ii=0; ii<Iter;ii++) {
		uint64_t timestamp = innoreal::InnoRealTimestamp::GetTimestamp();
		std::string timestamp_str = innoreal::InnoRealTimestamp::ConvertTimestampToString(timestamp);
		INNOREALI("%s\n", timestamp_str.c_str());
		innoreal::InnoRealTimer innoRealTimer;
		innoRealTimer.TimeStart();


		for(int i=0;i<K;i++){
			costL[i]=0;costR[i]=0;L[i]=0;R[i]=0;
		}

		for (int i = 0; i < center[0]; i++) {
			sprintf(file_src, "./data/168/mesh/mesh%d.obj", center[0]+startF);
			sprintf(file_tar, "./data/168/mesh/mesh%d.obj", i+startF);
			input_data->Init(file_src,file_tar);
			dis = (distmesh->*pmf)(input_data);
			label[i] = 1;
			costL[0] += dis;
		}
		L[0] = 0;

		for (int i = center.back(); i < N; i++) {
			sprintf(file_src, "./data/168/mesh/mesh%d.obj", center[K-1]+startF);
			sprintf(file_tar, "./data/168/mesh/mesh%d.obj", i+startF);
			input_data->Init(file_src,file_tar);
			INNOREALI("Start here:----------------%d\n",i);
			dis = (distmesh->*pmf)(input_data);
//          std::cout<<"KNN:"<<dis<<std::endl;

            label[i] = K;
			costR[K - 1] += dis;
		}
		std::cout<<std::endl;
		R[K - 1] = N - 1;

		for (int i = 0; i < K - 1; i++) {
			c1 = center[i];
			c2 = center[i + 1];
			sprintf(file_tar1, "./data/168/mesh/mesh%d.obj", c1+startF);
			sprintf(file_tar2, "./data/168/mesh/mesh%d.obj", c2+startF);
			for (int j = c1; j <= c2; j++) {
				sprintf(file_src, "./data/168/mesh/mesh%d.obj", j+startF);
				input_data->Init(file_src,file_tar1);
				dis1 = (distmesh->*pmf)(input_data);
				input_data->Init(file_src,file_tar2);
				dis2 = (distmesh->*pmf)(input_data);
				if (dis1 < dis2) {
					label[j] = i + 1;
					costR[i] += dis1;
					R[i] = j;
				} else {
					label[j] = i + 2;
					costL[i + 1] += dis2;
					if (L[i + 1] == 0) {
						L[i + 1] = j;
					}
				}
			}
		}

		d = 0;
		for (int i = 0; i < K; i++) {
          std::cout<<costL[i]<<","<<costR[i]<<"   ";
          d += costL[i] + costR[i];
		}
		cost.push_back(d);
		INNOREALI("cost: %lf\n", d);

		std::cout<<"Center:  ";
		for (int i = 0; i < K; i++) {
			center[i] = ceil((L[i]+1 + R[i]+1) / 2.0)-1;
			std::cout<<center[i]<<" ";
		}
		std::cout<<std::endl;

	  innoRealTimer.TimeEnd();
	  timestamp = innoreal::InnoRealTimestamp::GetTimestamp();
	  timestamp_str = innoreal::InnoRealTimestamp::ConvertTimestampToString(timestamp);
	  INNOREALI("%s\n", timestamp_str.c_str());
	  INNOREALI("%d iter time_ms: %f \n", ii, innoRealTimer.TimeGap_in_ms());

		for(int i=0;i<N;i++){
			std::cout<<label[i]<<" ";
		}
		std::cout<<std::endl;

	}


	std::vector<int> labelSeries;
	for(int i=0;i<N;i++){
		labelSeries.push_back(center[label[i]-1]+startF);
	}
	std::cout<<"save"<<std::endl;
//    kftD->saveVector("data/168/keyframe_csv2/costK", K, Iter, 1, cost);
//	  kftI->saveVector("data/168/keyframe_csv2/keyframeK", K, Iter, 1, labelSeries);
	kftD.saveVector("data/168/keyframe_csv2/costK", K, Iter, 1, cost);
	kftI.saveVector("data/168/keyframe_csv2/keyframeK", K, Iter, 1, labelSeries);


}

